/*
simple LED test
*/
#include <AFMotor.h>
#include <dht11.h>
#include "Ultrasonic.h"

#define DHT11PIN 36

const long TIME_TO_CHECK = 1000;

AF_DCMotor motor_1(1);
AF_DCMotor motor_2(2);
AF_DCMotor motor_3(3);
AF_DCMotor motor_4(4);

dht11 DHT11;
//Trigger - 46, Echo - 48
Ultrasonic ultrasonic(46, 48);

int sensorLightPin = 15;
int iterationNumber = 0;

unsigned long time = 0;



void setup() 
{
    time = millis();
    Serial.begin(115200); 
    Serial1.begin(9600);       // start serial communication at 9600bps
}
 
void loop() 
{
    
//-----------------------------------------------
//Gather information from Sensors
    if(millis() - time > TIME_TO_CHECK)
    {
      gatherInfo();
    }
    
    if(iterationNumber >= 4)
    {
      motor_1.run(RELEASE);
      motor_2.run(RELEASE);
      motor_3.run(RELEASE);
      motor_4.run(RELEASE);
      iterationNumber = 0;
    }
    
    iterationNumber++;
    if( Serial1.available() )       // if data is available to read
    {
      int leftVal = Serial1.parseInt();
      int rightVal = Serial1.parseInt();
      iterationNumber = 0;
      Serial.println(leftVal);
      Serial.println(rightVal);
      
      Serial.println("--------------------------------------------");
      
      if(leftVal < 0)
      {     
        motor_1.run(BACKWARD);
        motor_4.run(BACKWARD);
        motor_1.setSpeed(leftVal * (-1));
        motor_4.setSpeed(leftVal * (-1));
      }else
      {
        motor_1.run(FORWARD);
        motor_4.run(FORWARD);
        motor_1.setSpeed(leftVal);
        motor_4.setSpeed(leftVal);
      }
      if(rightVal < 0){     
        motor_2.run(BACKWARD);
        motor_3.run(BACKWARD);
        motor_2.setSpeed(rightVal * (-1));
        motor_3.setSpeed(rightVal * (-1));
      }else
      {
        motor_2.run(FORWARD);
        motor_3.run(FORWARD);
        motor_2.setSpeed(rightVal);
        motor_3.setSpeed(rightVal);
      }
      delay(200);
    }
}

void gatherInfo(void)
{
    time = millis();
    String message = "{";
    message += checkHumidityTemp();
    message += getDistanse(); 
    message += getLight();   
    message += "}";
      
    int lengthOfString = message.length() + 1;
    char buffer[lengthOfString];
    message.toCharArray(buffer,lengthOfString);
    Serial1.write(buffer);
}

String checkHumidityTemp()
{
    int chk = DHT11.read(DHT11PIN);
    Serial.print("Read sensor: ");
    switch (chk)
    {
        case 0: Serial.println("OK"); break;
        case -1: Serial.println("Checksum error"); break;
        case -2: Serial.println("Time out error"); break;
        default: Serial.println("Unknown error"); return;
    }

    String tempValue = String(DHT11.temperature);
    String humValue = String(DHT11.humidity);
  
    String finalString = "<Temperature:" + tempValue + ">"
                      + "<Humidity:" + humValue + ">";
                 
    return finalString;  
}

String getDistanse()
{
    int dist_cm = (int)ultrasonic.Ranging(CM); 	// get distance
    String distance = String(dist_cm);
    String finalString = "<distance:" + distance + ">";
    					// print the distance
    return finalString;
}

String getLight()
{
    int lightValue = analogRead(sensorLightPin);
    String stringValue = String(lightValue);
    String finalString = "<Light:" + stringValue + ">";
    Serial.println(finalString);
  
    return finalString;
}
